﻿#include <libv4l2.h>
#include <linux/videodev2.h>
#include <stdlib.h>
#include <string.h>
#include <fcntl.h>

#include <errno.h>
#include <assert.h>

#include <sys/mman.h>
#include <sys/ioctl.h>

#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>

#include "../../VideoReader.h"
#define IMAGEWIDTH_ 3264
#define IMAGEHEIGHT_ 2448


namespace pi {

struct buffer {
    void *                  start;
    size_t                  length;
};

struct ResolutionV4L
{
public:
    ResolutionV4L():width(0),height(0)
    {}

    ResolutionV4L(unsigned int width_,unsigned int height_)
        :width(width_),height(height_)
    {}

    unsigned int width;
    unsigned int height;
};

//////////////////////////////////////
/// \brief The VideoReaderImpl class
///
class VideoReaderImpl
{
public:
    VideoReaderImpl();
    ~VideoReaderImpl();
    int open(const std::string &name); // "/dev/video0"
    int close();

    std::string type(){ return "V4L2 video reader"; }

    int grabImage(VideoFrame &videoFrame);

    int call(const std::string &command, void *arg);
    int getOpt(const std::string &optName, std::string &opt);
    int setOpt(const std::string &optName, std::string opt);

protected:
    int                         fd;

    struct buffer *             buffers;
    unsigned int                n_buffers;

    ResolutionV4L               m_setRs;
    std::vector<ResolutionV4L>  supportList;
    int                         m_RsMode;

    //get support resolution
    int listSupportedResolution();
    int setupDevice();
    int startCaputre();
    int read_frame(VideoFrame &videoFrame);
};

VideoReaderImpl::VideoReaderImpl(){
    buffers = nullptr;
    n_buffers = 0;
    m_RsMode = 0;
}

VideoReaderImpl::~VideoReaderImpl(){
    ::close(fd);
}

int VideoReaderImpl::open(const std::string &name){
    fd = ::open(name.c_str(),O_RDWR);
    if (fd<0) {
        std::string info = "Error opening device: " + name;
        perror(info.c_str());
        exit (EXIT_FAILURE);
    }
    setupDevice();
}

int VideoReaderImpl::close(){
    ::close(fd);
}

int VideoReaderImpl::grabImage(VideoFrame &videoFrame){
    return read_frame(videoFrame);
}

int VideoReaderImpl::call(const std::string &command, void *arg){

}

int VideoReaderImpl::getOpt(const std::string &optName, std::string &opt){

}

int VideoReaderImpl::setOpt(const std::string &optName, std::string opt){

}

int VideoReaderImpl::listSupportedResolution()
{
    enum v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    struct v4l2_fmtdesc fmt_1;
    struct v4l2_frmsizeenum frmsize;

    fmt_1.index = 0;
    fmt_1.type = type;
    while (ioctl(fd, VIDIOC_ENUM_FMT, &fmt_1) >= 0) {
        frmsize.pixel_format = fmt_1.pixelformat;
        frmsize.index = 0;
        while (ioctl(fd, VIDIOC_ENUM_FRAMESIZES, &frmsize) >= 0){

            if(frmsize.type == V4L2_FRMSIZE_TYPE_DISCRETE){
                printf("No[%lu]: %dx%d\n",supportList.size(),frmsize.discrete.width, frmsize.discrete.height);
                supportList.push_back(ResolutionV4L(frmsize.discrete.width,frmsize.discrete.height));
            }

            frmsize.index++;
        }
        fmt_1.index++;
    }

    m_setRs = supportList[m_RsMode];
}


int VideoReaderImpl::setupDevice()
{
    int index;
    index = 0;
    if (-1 == ioctl (fd, VIDIOC_S_INPUT, &index)) {
        perror ("VIDIOC_S_INPUT");
        exit (EXIT_FAILURE);
    }

    struct v4l2_capability cap;   //显示设备信息
    struct v4l2_cropcap cropcap;  //设置摄像头的捕捉能力
    struct v4l2_crop crop;        //图像的缩放
    struct v4l2_format fmt;       //设置摄像头的视频制式、帧格式等

    if (-1 == ioctl (fd, VIDIOC_QUERYCAP, &cap)) {
        if (EINVAL == errno) {
            fprintf (stderr, "Device is no V4L2 device\n");
            exit (EXIT_FAILURE);
        } else {
            perror ("VIDIOC_QUERYCAP");
            exit(EXIT_FAILURE);
        }
    }

    if (!(cap.capabilities & V4L2_CAP_VIDEO_CAPTURE)) {
        fprintf (stderr, "Device is no video capture device\n");
        exit (EXIT_FAILURE);
    }


    if (!(cap.capabilities & V4L2_CAP_STREAMING)) {
        fprintf (stderr, "Device does not support streaming i/o\n");
        exit (EXIT_FAILURE);
    }

    memset(&cropcap, 0, sizeof(cropcap));

    cropcap.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;

    if (0 == ioctl (fd, VIDIOC_CROPCAP, &cropcap)) {
        crop.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
        crop.c = cropcap.defrect; /* reset to default */

        if (-1 == ioctl (fd, VIDIOC_S_CROP, &crop)) {
            switch (errno) {
            case EINVAL:
                /* Cropping not supported. */
                break;
            default:
                /* Errors ignored. */
                break;
            }
        }
    } else {
        /* Errors ignored. */
    }

    listSupportedResolution();

    memset(&fmt, 0, sizeof(fmt));
    fmt.type                = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    fmt.fmt.pix.width       = m_setRs.width;
    fmt.fmt.pix.height      = m_setRs.height;
    fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_MJPEG;
    fmt.fmt.pix.field       = V4L2_FIELD_INTERLACED;

    if (-1 == ioctl (fd, VIDIOC_S_FMT, &fmt)) {
        perror ("VIDIOC_S_FMT");
        exit(EXIT_FAILURE);
    }

    /* Note VIDIOC_S_FMT may change width and height. */

    //    /* Buggy driver paranoia. */
    //    min = fmt.fmt.pix.width * 2;
    //    if (fmt.fmt.pix.bytesperline < min)
    //        fmt.fmt.pix.bytesperline = min;

    //    min = fmt.fmt.pix.bytesperline * fmt.fmt.pix.height;

    //    if (fmt.fmt.pix.sizeimage < min)
    //        fmt.fmt.pix.sizeimage = min;

    //    printf("%d %d\n", fmt.fmt.pix.width, fmt.fmt.pix.height);
    //    printf("%d\n",fmt.fmt.pix.sizeimage);

    // Init mmap
    struct v4l2_requestbuffers req;

    memset(&req, 0, sizeof(req));

    req.count               = 4;
    req.type                = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    req.memory              = V4L2_MEMORY_MMAP;

    if (-1 == ioctl (fd, VIDIOC_REQBUFS, &req)) {
        if (EINVAL == errno) {
            fprintf (stderr, "Device does not support memory mapping\n");
            exit (EXIT_FAILURE);
        } else {
            perror ("VIDIOC_REQBUFS");
            exit(EXIT_FAILURE);
        }
    }

    if (req.count < 2) {
        fprintf (stderr, "Insufficient buffer memory on device\n");
        exit (EXIT_FAILURE);
    }

    buffers = (struct buffer*)calloc (req.count, sizeof (*buffers));

    if (!buffers) {
        fprintf (stderr, "Out of memory\n");
        exit (EXIT_FAILURE);
    }

    for (n_buffers = 0; n_buffers < req.count; ++n_buffers) {
        struct v4l2_buffer buf;

        memset(&buf, 0, sizeof(buf));

        buf.type        = V4L2_BUF_TYPE_VIDEO_CAPTURE;
        buf.memory      = V4L2_MEMORY_MMAP;
        buf.index       = n_buffers;

        if (-1 == ioctl (fd, VIDIOC_QUERYBUF, &buf)) {
            perror ("VIDIOC_QUERYBUF");
            exit(EXIT_FAILURE);
        }

        buffers[n_buffers].length = buf.length;
        buffers[n_buffers].start =
                mmap (NULL /* start anywhere */,
                      buf.length,
                      PROT_READ | PROT_WRITE /* required */,
                      MAP_SHARED /* recommended */,
                      fd, buf.m.offset);

        if (MAP_FAILED == buffers[n_buffers].start) {
            perror ("mmap");
            exit(EXIT_FAILURE);
        }
    }
    startCaputre();
}

int VideoReaderImpl::startCaputre()
{
    unsigned int i;
    enum v4l2_buf_type type;

    for (i = 0; i < n_buffers; ++i) {
        struct v4l2_buffer buf;

        memset(&buf, 0, sizeof(buf));

        buf.type        = V4L2_BUF_TYPE_VIDEO_CAPTURE;
        buf.memory      = V4L2_MEMORY_MMAP;
        buf.index       = i;

        if (-1 == ioctl (fd, VIDIOC_QBUF, &buf)) {
            perror ("VIDIOC_QBUF");
            exit(EXIT_FAILURE);
        }
    }

    type = V4L2_BUF_TYPE_VIDEO_CAPTURE;

    if (-1 == ioctl (fd, VIDIOC_STREAMON, &type)) {
        perror ("VIDIOC_STREAMON");
        exit(EXIT_FAILURE);
    }
}

int VideoReaderImpl::read_frame(VideoFrame &videoFrame)
{
    struct v4l2_buffer  buf;
    memset(&buf, 0, sizeof(buf));

    buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    buf.memory = V4L2_MEMORY_MMAP;

    if (-1 == ioctl (fd, VIDIOC_DQBUF, &buf)) {
        switch (errno) {
        case EAGAIN:
            return 0;

        case EIO:
            /* Could ignore EIO, see spec. */

            /* fall through */

        default:
            perror ("VIDIOC_DQBUF");
            exit(EXIT_FAILURE);
        }
    }

    assert (buf.index < n_buffers);

    /////////////////////////////////////////////////////
    /// alloc memory and record data
    videoFrame.height = m_setRs.height;
    videoFrame.width  = m_setRs.width;
    videoFrame.channel = 3;

    if (-1 == ioctl (fd, VIDIOC_QBUF, &buf)) {
        perror ("VIDIOC_QBUF");
        exit(EXIT_FAILURE);
    }
    CvMat cvmat = cvMat(IMAGEHEIGHT_,IMAGEWIDTH_,CV_8UC3,buffers[buf.index].start);
    IplImage* img = cvDecodeImage(&cvmat,1);
    if(!img){
        printf("DecodeImage error!\n");
    }
    cv::Mat src(img);
    static size_t bufSize = videoFrame.height * videoFrame.width * videoFrame.channel * sizeof(uchar);

    if(videoFrame.img == nullptr){
        videoFrame.img = new uchar[bufSize];
    }
    memcpy(videoFrame.img,src.data,bufSize);
    //    cv::imshow("Mat",src);
    //    cvShowImage("Capture",img);
    cvReleaseImage(&img);
    //    cvWaitKey(1);
    //    struct v4l2_buffer queue_buf;
    //    memset(&queue_buf, 0, sizeof(queue_buf));
    //    queue_buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    //    queue_buf.memory = V4L2_MEMORY_MMAP;
    //    if (-1 == ioctl(fd, VIDIOC_QBUF, &queue_buf)) {
    //        printf("VIDIOC_QBUF error\n");
    //        return -1;
    //    }

    return 1;
}


////////////////////////////////////////////////////////////////////
///
VideoReader::VideoReader():impl(SPtr<VideoReaderImpl>(new VideoReaderImpl))
{

}

VideoReader::~VideoReader()
{
    close();
}

int VideoReader::open(const std::string &name)
{
    return impl->open(name);
}

int VideoReader::close()
{
    return impl->close();
}

std::string VideoReader::type()
{
    return impl->type();
}

int VideoReader::call(const std::string &command, void *arg)
{
    return impl->call(command,arg);
}

int VideoReader::grabImage(VideoFrame &videoFrame)
{
    return impl->grabImage(videoFrame);
}

int VideoReader::getOpt(const std::string &optName, std::string &opt)
{
    return impl->getOpt(optName,opt);
}

int VideoReader::setOpt(const std::string &optName, std::string opt)
{
    return impl->setOpt(optName,opt);
}
} //end of namespace pi


SPtr<pi::VideoReader> getVideoReader()
{
    return SPtr<pi::VideoReader>(new pi::VideoReader());
}
